#include "bsp_icm20608.h"
#include "bsp_gpio.h"
#include "bsp_spi.h"
#include "bsp_delay.h"
#include "stdio.h"

/* 初始化ICM20608 */
void icm20608_init(void)
{
	gpio_pin_config_t config;

	/* 1.SPI 引脚的初始化 */
	IOMUXC_SetPinMux(IOMUXC_UART2_RX_DATA_ECSPI3_SCLK, 0);				/* 复用为ECSPI3_SCLK */
	IOMUXC_SetPinMux(IOMUXC_UART2_CTS_B_ECSPI3_MOSI, 0);				/* 复用为ECSPI3_MOSI */
	IOMUXC_SetPinMux(IOMUXC_UART2_RTS_B_ECSPI3_MISO, 0);				/* 复用为ECSPI3_MISO */


	IOMUXC_SetPinConfig(IOMUXC_UART2_RX_DATA_ECSPI3_SCLK, 0x10B1);		/* IO特性 */
	IOMUXC_SetPinConfig(IOMUXC_UART2_CTS_B_ECSPI3_MOSI, 0x10B1);		/* IO特性 */
	IOMUXC_SetPinConfig(IOMUXC_UART2_RTS_B_ECSPI3_MISO, 0x10B1);		/* IO特性 */


	IOMUXC_SetPinMux(IOMUXC_UART2_TX_DATA_GPIO1_IO20, 0);				/* 复用为GPIO1_20 */
	IOMUXC_SetPinConfig(IOMUXC_UART2_TX_DATA_GPIO1_IO20, 0x10B0);		/* IO特性 */
	config.direction = kGPIO_DigitalOutput;
	config.outputLogic = 0;
	gpio_init(GPIO1, 20, &config);


	/* 2.SPI控制器的初始化 */
	spi_init(ECSPI3);


	/* 3.icm20608的初始化 */
	/* 睡眠模式，手册中说icm20608复位之后默认是sleep模式 */
	icm20608_write_reg(ECSPI3, ICM20608_REG_PWR_MGMT_1, 0x80);				/* 复位 */
	delayms(50);
	icm20608_write_reg(ECSPI3, ICM20608_REG_PWR_MGMT_1, 0x01);				/* 关闭睡眠模式，时钟选择 */
	delayms(50);

	unsigned char value;
	value = icm20608_read_reg(ECSPI3, ICM20608_REG_WHO_AM_I);
	printf("ICM20608G ICM20608_REG_WHO_AM_I=0x%02x\r\n", value);

	icm20608_write_reg(ECSPI3, ICM20608_REG_SMLPRT_DIV, 0x0);				/* 设置采样率输出速率 */
	icm20608_write_reg(ECSPI3, ICM20608_REG_GYRO_CONFIG, 0x18);				/* 陀螺仪量程FS(Full Scale), ±2000dps*/
	icm20608_write_reg(ECSPI3, ICM20608_REG_ACCEL_CONFIG, 0x18);			/* 加速度计量程FS(Full Scale), ±16g */
	icm20608_write_reg(ECSPI3, ICM20608_REG_CONFIG, 0x04);					/* 陀螺仪, BW=20Hz, 低通滤波器DLPF(Digital Low Pass Filter) */
	icm20608_write_reg(ECSPI3, ICM20608_REG_ACCEL_CONFIG2, 0x04);			/* 加速度, BW=20Hz, 低通滤波器DLPF(Digital Low Pass Filter) */
	icm20608_write_reg(ECSPI3, ICM20608_REG_PWR_MGMT_2, 0x00);				/* 启用所有陀螺仪，加速度计 X,Y,Z轴 */
	icm20608_write_reg(ECSPI3, ICM20608_REG_LP_MODE_CFG, 0x00);				/* 关闭低功耗模式，陀螺仪，加速度计的低功耗模式 */
	icm20608_write_reg(ECSPI3, ICM20608_REG_FIFO_EN, 0x00);					/* 关闭FIFO，陀螺仪，加速度计，温度计，FIFO使能 */

}

unsigned char icm20608_read_reg(ECSPI_Type *base, unsigned char reg)
{
	unsigned char rxdata = 0;

	reg |= 0x80;									/* icmp20608 读，最高位为 1 */	
	
	ICM20608_CSN(0);								/* 拉低SPI CS片选信号，选中从设备 */
	spi_readwrite_onebyte(base, reg);				/* icm20608读,寄存器地址 */	
	rxdata = spi_readwrite_onebyte(base, 0xFF);		/* icm20608读,dummy数据*/
	ICM20608_CSN(1);								/* 拉高SPI CS片选信号，去选中从设备 */

	return rxdata;
}

void icm20608_write_reg(ECSPI_Type *base, unsigned char reg, unsigned char value)
{
	reg &= ~0x80;							/* icmp20608 写，最高位清零 */	

	ICM20608_CSN(0);						/* 拉低SPI CS片选信号，选中从设备 */
	spi_readwrite_onebyte(base, reg);		/* icm20608写,寄存器地址 */	
	spi_readwrite_onebyte(base, value);		/* icm20608写,数据*/
	ICM20608_CSN(1);						/* 拉高SPI CS片选信号，去选中从设备 */
}

void icm20608_read_len(ECSPI_Type *base, unsigned char reg, unsigned char len, unsigned char *buf)
{
	int i  = 0;

	// for(i=0; i< len; i++)
	// {
	// 	buf[i] = icm20608_read_reg(base, reg+i);
	// }

	reg |= 0x80;									/* icmp20608 读，最高位为 1 */	
	
	ICM20608_CSN(0);								/* 拉低SPI CS片选信号，选中从设备 */
	spi_readwrite_onebyte(base, reg);				/* icm20608读,寄存器地址 */	
	/* 连续读取多个数据 */
	for(i=0; i<len; i++){
		buf[i] = spi_readwrite_onebyte(base, 0xFF);	/* icm20608读,dummy数据*/
	}
	ICM20608_CSN(1);								/* 拉高SPI CS片选信号，去选中从设备 */

}


void icm20608_read_dev_accel_gyro(struct icm20608_device *idev)
{
	unsigned char buf[14];
	int gyroscale;
	int accescale;

	icm20608_read_len(ECSPI3, ICM20_ACCEL_XOUT_H, 14, buf);

	idev->accel_x_adc = (signed short)((buf[0] << 8) | buf[1]);
	idev->accel_y_adc = (signed short)((buf[2] << 8) | buf[3]);
	idev->accel_z_adc = (signed short)((buf[4] << 8) | buf[5]);

	idev->temperature_adc =  buf[6] << 8;
	idev->temperature_adc |= buf[7];

	idev->gyro_x_adc =  (signed short)((buf[8] << 8) | buf[9]);
	idev->gyro_y_adc =  (signed short)((buf[10] << 8) | buf[11]);
	idev->gyro_z_adc =  (signed short)((buf[12] << 8) | buf[13]);

	gyroscale = icm20608_gyroscale_get();
	accescale = icm20608_accelscale_get();

	/* 乘上100 用来保存小数，用的时候再除以100 */
	idev->accel_x_act = (((signed int)idev->accel_x_adc * accescale)*100)/65536;
	idev->accel_y_act = (((signed int)idev->accel_y_adc * accescale)*100)/65536;
	idev->accel_z_act = (((signed int)idev->accel_z_adc * accescale)*100)/65536;
	idev->gyro_x_act = (((signed int)idev->gyro_x_adc * gyroscale)*100)/65536;
	idev->gyro_y_act = (((signed int)idev->gyro_y_adc * gyroscale)*100)/65536;
	idev->gyro_z_act = (((signed int)idev->gyro_z_adc * gyroscale)*100)/65536;
}


/*
 * @description 	: 获取陀螺仪分辨率（量程）。
 * @param  			: 无
 * @return 			: 无
 */
int icm20608_gyroscale_get(void)
{
	unsigned char rxdata;
	int gyroScale = 0;

	rxdata = icm20608_read_reg(ECSPI3, ICM20608_REG_GYRO_CONFIG);

	switch(((rxdata >> 3) & 0x3))
	{
		case 0:
			gyroScale = 500;			//gyro_scale ±250dps
			break;
		case 1:
			gyroScale = 1000;			//gyro_scale ±500dps
			break;
		case 2:
			gyroScale = 2000;			//gyro_scale ±1000dps
			break;
		case 3:
			gyroScale = 4000;			//gyro_scale ±2000dps
			break;
	}

	printf("%s() rxdata=%02x gyroScale=%d\r\n", __FUNCTION__, rxdata, gyroScale);
	return gyroScale;
}



/*
 * @description 	: 获取加速度计分辨率（量程）。
 * @param  			: 无
 * @return 			: 无
 */
int icm20608_accelscale_get(void)
{
	unsigned char rxdata;
	int accelScale;

	rxdata = icm20608_read_reg(ECSPI3, ICM20608_REG_ACCEL_CONFIG);

	//switch(((rxdata & 0x3) >> 3))
	switch(((rxdata >> 3) & 0x3))
	{
		case 0:
			accelScale = 4;			//accel_scale ±2g
			break;
		case 1:
			accelScale = 8;			//accel_scale ±4g
			break;
		case 2:
			accelScale = 16;		//accel_scale ±8g
			break;
		case 3:
			accelScale = 32;		//accel_scale ±16g
			break;
	}
	
	printf("%s() rxdata=%02x accelScale=%d\r\n", __FUNCTION__, rxdata, accelScale);
	return accelScale;
}